Imports MatrixDataStructures
Imports twoDMatrixLibrary


<System.Runtime.InteropServices.ComVisible(False)> Public Class VolumeRegistrationByMutualInformation

#Region "Variable Declarations"
    Dim alpha, beta, gamma, sigma As Double
    'reflection, contraction, expansion & shrinking coefficients for Search
    Private X, Y, Z As Integer
    'for mutual information calculation
    Private TemplateMatrix, MovingMatrix As Matrix3DInt16
    Public Log As New ArrayList
    Dim BestVector As Vector
    Dim GoodVector As Vector
    Dim WorstVector As Vector
#End Region


    Sub New(ByVal X As Integer, ByVal Y As Integer, _
    Optional ByVal Z As Integer = 0, _
    Optional ByVal alpha As Double = 1, _
    Optional ByVal beta As Double = 1, _
    Optional ByVal gamma As Double = 0.5, _
    Optional ByVal sigma As Double = 0.5)
        'Since twoDMatrixLibrary.Matrix size will remain the same during the whole problem, we can set 
        'Matrix size at the beginning
        Me.X = X
        Me.Y = Y
        Me.Z = Z

        Me.alpha = alpha
        Me.beta = beta
        Me.gamma = gamma
        Me.sigma = sigma

    End Sub

    Function PrintParameters() As String
        Return "(" & alpha & "," & beta & "," & gamma & "," & sigma & ")"
    End Function
    Private Function ComputeJointHistogram(ByVal MatrixA As Matrix3DInt16, ByVal MatrixB As Matrix3DInt16 _
    , Optional ByVal Normalized As Boolean = False) As Matrix
        Dim AMin, BMin, AMax, BMax As Int16
        AMin = MatrixA.Minimum
        BMin = MatrixB.Minimum
        AMax = MatrixA.Maximum
        BMax = MatrixB.Maximum
        Dim JointHistogramMatrix As Matrix
        JointHistogramMatrix = New Matrix(AMax - AMin + 1, BMax - BMin + 1)
        For i As Integer = 0 To X - 1
            For j As Integer = 0 To Y - 1
                For k As Integer = 0 To Z - 1
                    JointHistogramMatrix(MatrixA.data(i, j, k) - AMin, MatrixB.data(i, j, k) - BMin) += 1

                Next
            Next
        Next
        Dim Total As Double = X * Y * Z 'differnt type of normalization
        'JointHistogramMatrix.SaveAs("c:\temp1.txt")
        If Normalized = True Then
            'normalize the histogram
            For i As Integer = 0 To JointHistogramMatrix.Rows - 1
                For j As Integer = 0 To JointHistogramMatrix.Columns - 1
                    JointHistogramMatrix(i, j) = _
                    JointHistogramMatrix(i, j) / Total
                Next
            Next
        End If
        JointHistogramMatrix.SaveAs("c:\temp2.txt")
        Return JointHistogramMatrix
    End Function
    Private Function ComputeJointHistogram(ByVal MatrixA As Matrix, ByVal MatrixB As twoDMatrixLibrary.Matrix _
    , Optional ByVal Normalized As Boolean = False) As twoDMatrixLibrary.Matrix

        'rows=size(image_1,1);
        'cols=size(image_1,2);
        'N=256;

        'h=zeros(N,N);

        'for i=1:rows;    %  col 
        '  for j=1:cols;   %   rows
        '    h(image_1(i,j)+1,image_2(i,j)+1)= h(image_1(i,j)+1,image_2(i,j)+1)+1;
        '  End
        'End
        Dim JointHistogramMatrix As twoDMatrixLibrary.Matrix
        JointHistogramMatrix = New twoDMatrixLibrary.Matrix(256, 256, 0)
        For i As Integer = 0 To X - 1
            For j As Integer = 0 To Y - 1
                JointHistogramMatrix(CInt(MatrixA(i, j)), CInt(MatrixB(i, j))) = _
                JointHistogramMatrix(CInt(MatrixA(i, j)), CInt(MatrixB(i, j))) + 1
            Next
        Next
        Dim Total As Double = JointHistogramMatrix.Rows * JointHistogramMatrix.Columns
        If Normalized = True Then
            'normalize the histogram
            For i As Integer = 0 To JointHistogramMatrix.Rows - 1
                For j As Integer = 0 To JointHistogramMatrix.Columns - 1
                    JointHistogramMatrix(i, j) = _
                    JointHistogramMatrix(i, j) / Total
                Next
            Next
        End If
        Return JointHistogramMatrix

    End Function
    Public Function ComputeMutualInformation(ByRef MatrixA As Matrix3DInt16, ByRef MatrixB As Matrix3DInt16) As Double
        Dim JointHistogram As twoDMatrixLibrary.Matrix = ComputeJointHistogram(MatrixA, MatrixB, True)
        Dim RowSize As Integer = JointHistogram.Rows
        Dim ColSize As Integer = JointHistogram.Columns

        'compute Marginals
        Dim YMarginal(ColSize) As Double
        Dim XMarginal(RowSize) As Double

        'X Marginal is sum AS rows
        'Y Marginal is sum AS cols

        For i As Integer = 0 To RowSize - 1
            For j As Integer = 0 To ColSize - 1
                XMarginal(i) += JointHistogram(i, j)
            Next
        Next

        For j As Integer = 0 To ColSize - 1
            For i As Integer = 0 To RowSize - 1
                YMarginal(j) += JointHistogram(i, j)
            Next
        Next

        Dim Hx, Hy, H_xy As Double

        For i As Integer = 0 To RowSize - 1
            If XMarginal(i) <> 0 Then
                Hx += (-1 * XMarginal(i) * Math.Log(XMarginal(i), 2))
            End If
        Next
        For j As Integer = 0 To ColSize - 1
            If YMarginal(j) <> 0 Then
                Hy += (-1 * YMarginal(j) * Math.Log(YMarginal(j), 2))
            End If
        Next

        'h_xy = -sum(sum(b.*(log2(b+(b==0))))); % joint entropy

        For j As Integer = 0 To ColSize - 1
            For i As Integer = 0 To RowSize - 1
                H_xy = H_xy + (-1 * JointHistogram(i, j) * Math.Log(JointHistogram(i, j) + CDbl(IIf(JointHistogram(i, j) = 0, 1, 0)), 2))
            Next
        Next
        Return -1 * (Hx + Hy - H_xy)
    End Function
    Public Function ComputeMutualInformation(ByRef MatrixA As twoDMatrixLibrary.Matrix, ByRef MatrixB As twoDMatrixLibrary.Matrix) As Double
        Dim JointHistogram As twoDMatrixLibrary.Matrix = ComputeJointHistogram(MatrixA, MatrixB, True)
        Dim RowSize As Integer = JointHistogram.Rows
        Dim ColSize As Integer = JointHistogram.Columns

        'compute Marginals
        Dim YMarginal(ColSize) As Double
        Dim XMarginal(RowSize) As Double

        'X Marginal is sum AS rows
        'Y Marginal is sum AS cols

        For i As Integer = 0 To RowSize - 1
            For j As Integer = 0 To ColSize - 1
                XMarginal(i) += JointHistogram(i, j)
            Next
        Next

        For j As Integer = 0 To ColSize - 1
            For i As Integer = 0 To RowSize - 1
                YMarginal(j) += JointHistogram(i, j)
            Next
        Next

        Dim Hx, Hy, H_xy As Double

        For i As Integer = 0 To RowSize - 1
            If XMarginal(i) <> 0 Then
                Hx += (-1 * XMarginal(i) * Math.Log(XMarginal(i), 2))
            End If
        Next
        For j As Integer = 0 To ColSize - 1
            If YMarginal(j) <> 0 Then
                Hy += (-1 * YMarginal(j) * Math.Log(YMarginal(j), 2))
            End If
        Next

        'h_xy = -sum(sum(b.*(log2(b+(b==0))))); % joint entropy

        For j As Integer = 0 To ColSize - 1
            For i As Integer = 0 To RowSize - 1
                H_xy = H_xy + (-1 * JointHistogram(i, j) * Math.Log(JointHistogram(i, j) + CDbl(IIf(JointHistogram(i, j) = 0, 1, 0)), 2))
            Next
        Next
        Return -1 * (Hx + Hy - H_xy)
    End Function

#Region "Nelder Mead Minima Finding"
#Region "Expansion Contraction etc. functions"
    Private Function Reflect(ByVal MidPoint As Vector, ByVal WorstVector As Vector) As Vector
        Dim ReflectedPoint As New Vector(MidPoint + (MidPoint - WorstVector) * alpha)
        Return ReflectedPoint
    End Function
    Private Function Expand(ByVal MidPoint As Vector, ByVal ReflectionPoint As Vector) As Vector
        Dim ExpandedPoint As New Vector(ReflectionPoint + (ReflectionPoint - MidPoint) * beta)
        Return ExpandedPoint
    End Function
    Private Function Contract(ByVal MidPoint As Vector, ByVal WorstVector As Vector, ByVal ReflectedPoint As Vector) As Vector
        Dim ContractedPoint1 As New Vector(MidPoint + (WorstVector - MidPoint) * gamma)
        Dim ContractedPoint2 As New Vector(MidPoint + (ReflectedPoint - MidPoint) * gamma)
        If GetFunctionValue(ContractedPoint1) > GetFunctionValue(ContractedPoint2) Then
            Return ContractedPoint1
        Else
            Return ContractedPoint2
        End If
    End Function
#End Region
#Region "Order, Shrink & Swap & MidPoint"
    Private Function Order(ByVal VectorArray As Vector()) As Vector()
        Dim SizeofArray As Integer = VectorArray.Length
        Dim FunctionValues(SizeofArray - 1) As Double
        'this will speed things up
        For i As Integer = 0 To SizeofArray - 1
            FunctionValues(i) = GetFunctionValue(VectorArray(i))
        Next
        'Bubble Sort Vector array
        Me.BubbleSort(FunctionValues, VectorArray)
        Return VectorArray
    End Function
#Region "Bubble Sort"
    Private Sub BubbleSort(ByRef arrayBeingSorted() As Double, _
                          Optional ByRef tagArray() As Vector = Nothing, _
                          Optional ByVal includeZero As Boolean = True)
        '-------------------------------------------------------------------------
        ' Sort an array into ascending sequence
        ' (works best on arrays with 1000 elements or less).
        '
        ' Pass:   arrayBeingSorted    Self explanatory (when a string array,
        '                             the sort is case sensitive)
        '         tagArray            Optional second array of strings that,
        '                             when passed, will be reordered in concert
        '                             with the array being sorted (the tagArray
        '                             must have the same number of elements as
        '                             the array being sorted)
        '         includeZero         True  = Include 0 element in sort
        '                             False = Do not include 0 element in sort
        ' Return: arrayBeingSorted    In prescribed sequence
        '         tagArray            When included, reordered to match the
        '                             arrayBeingSorted
        '-------------------------------------------------------------------------
        '     Date    Developer            Comments
        '  ---------- -------------------- ---------------------------------------
        '  12/19/2005 G Gilbert            Original code
        '-------------------------------------------------------------------------

        '------------------------------------------------
        ' Local Constant/Variable Declarations
        '------------------------------------------------
        Dim FirstElement As Integer
        Dim LastElement As Integer
        Dim NextPointer As Integer
        Dim NoSwapMade As Boolean
        Dim PassNumber As Integer
        Dim SortTags As Boolean

        '------------------------------------------------
        ' Set the lower and upper bounds of the sort
        '------------------------------------------------
        If Not includeZero Then
            FirstElement = 1
        End If
        LastElement = arrayBeingSorted.GetUpperBound(0)

        '------------------------------------------------
        ' Determine if an array of tags was passed.
        ' When passed, the length of the tag
        ' array must be the same as the array
        ' being sorted. If they are not the same
        ' length, the tag array is not sorted.
        '------------------------------------------------
        If Not IsNothing(tagArray) Then
            If tagArray.GetUpperBound(0) = LastElement Then
                SortTags = True
            End If
        End If

        '------------------------------------------------
        ' Arrays with none, or only one
        ' element don't need sorting
        '------------------------------------------------
        If LastElement <= FirstElement Then
            Exit Sub
        End If

        '------------------------------------------------
        ' A bubble sort will not work on arrays 
        ' of less than 3 elements. When there are
        ' only two elements, do a quick comparison
        ' and, if needed, swap the array
        ' elements.
        '------------------------------------------------
        If (LastElement - FirstElement + 1) = 2 Then
            If arrayBeingSorted(FirstElement) > _
                   arrayBeingSorted(LastElement) Then
                swap(arrayBeingSorted(FirstElement), _
                             arrayBeingSorted(LastElement))
                If SortTags Then
                    swap(tagArray(FirstElement), _
                                  tagArray(LastElement))
                End If
            End If
            Exit Sub
        End If

        '------------------------------------------------
        ' Do the bubble sort (stop the sort when
        ' a pass is made with no swaps)
        '------------------------------------------------
        For PassNumber = 0 To LastElement
            NoSwapMade = True
            For NextPointer = 0 To (LastElement - 1) - PassNumber
                If arrayBeingSorted(NextPointer) > _
                          arrayBeingSorted(NextPointer + 1) Then
                    swap(arrayBeingSorted(NextPointer), _
                                 arrayBeingSorted(NextPointer + 1))
                    If SortTags Then
                        swap(tagArray(NextPointer), _
                                      tagArray(NextPointer + 1))
                    End If
                    NoSwapMade = False
                End If
            Next
            If NoSwapMade Then
                Exit For
            End If
        Next

    End Sub

#End Region
    Private Sub swap(ByRef A As Double, ByRef B As Double)
        Dim temp As Double = A
        B = A
        A = temp
    End Sub
    Private Sub Swap(ByRef A As Vector, ByRef B As Vector)
        Dim tempv As New Vector(B)
        B = New Vector(A)
        A = New Vector(tempv)
    End Sub
    Private Sub Shrink(ByRef GoodPoint As Vector, ByRef BestPoint As Vector, ByRef WorstPoint As Vector, ByRef MidPoint As Vector)
        GoodPoint = New Vector(MidPoint)
        WorstPoint = BestPoint + (WorstPoint - BestPoint) * sigma
    End Sub
    Private Function ComputeMidPoint(ByVal BestVector As Vector, ByVal GoodVector As Vector) As Vector
        Return (BestVector + GoodVector) / 2.0#
    End Function
#End Region
    Public Function ComputeMinimum(ByVal StartingVector1 As Vector, _
        ByVal StartingVector2 As Vector, ByVal StartingVector3 As Vector, _
        ByVal VectorMaxValues As Vector, ByVal MaxCycles As Integer, _
        ByVal StaticM As Matrix3DInt16, ByVal MovingM As Matrix3DInt16) As Vector

        Me.TemplateMatrix = StaticM
        Me.MovingMatrix = MovingM
        'sort
        Dim VectorArray(2) As Vector
        VectorArray(0) = StartingVector1
        VectorArray(1) = StartingVector2
        VectorArray(2) = StartingVector3

        VectorArray = Order(VectorArray)

        'MaxValues
        BestVector = VectorArray(0)
        GoodVector = VectorArray(1)
        WorstVector = VectorArray(2)

        Dim ReflectionPoint, ContractionPoint, ExpansionPoint, MidPoint As Vector
        Dim ReflectionPointValue, ContractionPointValue, ExpansionPointValue As Double
        Dim BestVectorvalue, GoodVectorValue, WorstVectorValue As Double

        Dim CycleCounter As Integer = 0

        While CycleCounter < MaxCycles 'WorstVector < VectorMaxValues And BestVector < VectorMaxValues And GoodVector < VectorMaxValues And 

            CycleCounter += 1

            'start and continue
            VectorArray(0) = BestVector
            VectorArray(1) = GoodVector
            VectorArray(2) = WorstVector

            VectorArray = (Order(VectorArray))
            BestVector = VectorArray(0)
            GoodVector = VectorArray(1)
            WorstVector = VectorArray(2)
            BestVectorvalue = GetFunctionValue(BestVector)
            GoodVectorValue = GetFunctionValue(GoodVector)
            WorstVectorValue = GetFunctionValue(WorstVector)

            Dim A(2) As String
            A(0) = WorstVector.ToString
            A(1) = CStr(WorstVectorValue)
            Log.Add(A)

            If BestVectorvalue = WorstVectorValue And BestVectorvalue = GoodVectorValue And CycleCounter > 2 Then
                Exit While
            End If
            MidPoint = Me.ComputeMidPoint(BestVector, GoodVector)
            ReflectionPoint = Me.Reflect(MidPoint, WorstVector)
            ReflectionPointValue = GetFunctionValue(ReflectionPoint)

            If ReflectionPointValue <= GoodVectorValue Then
                If ReflectionPointValue > BestVectorvalue Then
                    WorstVector = New Vector(ReflectionPoint)
                Else
                    ExpansionPoint = Me.Expand(MidPoint, ReflectionPoint)
                    ExpansionPointValue = Me.GetFunctionValue(ExpansionPoint)

                    If ExpansionPointValue < BestVectorvalue Then
                        WorstVector = New Vector(ExpansionPoint)
                    Else
                        WorstVector = New Vector(ReflectionPoint)
                    End If
                End If
            Else
                If ReflectionPointValue < WorstVectorValue Then
                    WorstVector = New Vector(ReflectionPoint)
                End If
                ContractionPoint = Me.Contract(MidPoint, WorstVector, ReflectionPoint)
                ContractionPointValue = Me.GetFunctionValue(ContractionPoint)

                If ContractionPointValue < WorstVectorValue Then
                    WorstVector = ContractionPoint
                Else
                    Shrink(GoodVector, BestVector, WorstVector, MidPoint)
                End If
            End If

            'and sort
        End While
        VectorArray(0) = BestVector
        VectorArray(1) = GoodVector
        VectorArray(2) = WorstVector

        VectorArray = (Order(VectorArray))
        BestVector = VectorArray(0)
        GoodVector = VectorArray(1)
        WorstVector = VectorArray(2)
        BestVectorvalue = GetFunctionValue(BestVector)
        GoodVectorValue = GetFunctionValue(GoodVector)
        WorstVectorValue = GetFunctionValue(WorstVector)

        Dim s(2) As String
        s(0) = "Iterations for calculation of "
        s(1) = CStr(CycleCounter - 1)
        Log.Add(s)
        Return BestVector
    End Function
#End Region

#Region "Simulated Annealing"
    Dim RandomGeneratorClass As New System.Random

    Public Function ComputeAnnealingMinimum(ByVal MaxVector As Vector, _
            ByVal StaticM As Matrix3DInt16, ByVal MovingM As Matrix3DInt16, _
            Optional ByVal Temperature As Single = 1000, _
            Optional ByVal NumberOfSteps As Integer = 50, _
            Optional ByVal TemperatureDecrement As Single = 0.95, _
            Optional ByVal TemperatureCutoff As Single = 1) As Vector

        Me.TemplateMatrix = StaticM
        Me.MovingMatrix = MovingM

        Dim NumofMonteCarloSteps As Integer = 0

        'Compute a Random Initial Vector
        Dim InitialVector As New Vector(MaxVector.NumberOfDimensions)
        For i As Integer = 0 To InitialVector.NumberOfDimensions - 1
            InitialVector(i) = MaxVector(0) * RandomGeneratorClass.NextDouble * RandomGeneratorClass.Next(-1, 1)
        Next
        Dim NeighbourVector As New Vector(MaxVector.NumberOfDimensions)
        Dim InitialVectorValue, NeighbourVectorValue As Double
        While Temperature > TemperatureCutoff
            For i As Integer = 0 To NumberOfSteps - 1

                'Compute a random neighbour vector to initial vector
                For j As Integer = 0 To InitialVector.NumberOfDimensions - 1
                    NeighbourVector(j) = InitialVector(0) + RandomGeneratorClass.NextDouble * RandomGeneratorClass.Next(-1, 1)
                Next

                'Evaluate at these two vectors
                InitialVectorValue = Me.GetFunctionValue(InitialVector)
                NeighbourVectorValue = Me.GetFunctionValue(NeighbourVector)

                If InitialVectorValue > NeighbourVectorValue Then
                    InitialVector = New Vector(NeighbourVector) ' greedy step
                Else 'metropolis step
                    If Math.E ^ (-1 * (NeighbourVectorValue - InitialVectorValue) / Temperature) > RandomGeneratorClass.NextDouble Then
                        InitialVector = New Vector(NeighbourVector)
                    End If
                End If
                NumofMonteCarloSteps += 1
            Next
            MsgBox(Temperature & " " & InitialVector.ToString)
            Temperature *= TemperatureDecrement
        End While
        Return InitialVector
    End Function
#End Region

    'TEMP FUNCTION
    Private Function GetFunctionValue(ByVal Point As Vector) As Double
        Dim a As Single = CSng(Point(0))
        Dim b As Single = CSng(Point(1))
        Dim c As Single = CSng(Point(2))
        Dim d As Single = CSng(Point(3))
        Dim e As Single = CSng(Point(4))
        Dim f As Single = CSng(Point(5))
        Return Me.ComputeMutualInformation(TemplateMatrix, MovingMatrix.RotateAndTranslateFunction(a, b, c, d, e, f))
    End Function
    Public Sub ApplyMinimum(ByRef Matrix As Matrix3DInt16)
        Matrix.RotateAndTranslate(CSng(BestVector(0)), CSng(BestVector(1)), CSng(BestVector(2)), CSng(BestVector(3)), CSng(BestVector(4)), CSng(BestVector(5)))
    End Sub
    Public Sub ApplyMinimum(ByRef Matrix As Matrix3DSingle)
        Matrix.RotateAndTranslate(CSng(BestVector(0)), CSng(BestVector(1)), CSng(BestVector(2)), CSng(BestVector(3)), CSng(BestVector(4)), CSng(BestVector(5)))
    End Sub
End Class
